import pysoem
import time

class EtherCATDriver:
    def __init__(self, ifname='eth0'):
        self.master = pysoem.Master()
        self.ifname = ifname
        self.master.in_op = False
        self.master.do_check_state = False

    def setup(self):
        """Initialize EtherCAT master and configure slaves"""
        try:
            self.master.open(self.ifname)
            
            # Configure DC for synchronization
            self.master.config_dc()
            
            # Check if slaves are found
            if self.master.config_init() > 0:
                self.master.config_map()
                self.master.state_check(pysoem.SAFEOP_STATE, 50000)
                self.master.state_check(pysoem.OP_STATE, 50000)
                self.master.write_state()
                self.master.do_check_state = True
                self.master.in_op = True
                return True
            return False
        except Exception as e:
            print(f"Setup failed: {str(e)}")
            return False

    def read_feedback(self):
        """Read motor feedback data"""
        try:
            if self.master.in_op:
                self.master.send_processdata()
                self.master.receive_processdata(2000)
                
                # Read feedback from first slave (adjust index as needed)
                slave = self.master.slaves[0]
                
                # Example: reading position feedback (adjust offset based on your PDO mapping)
                position = int.from_bytes(
                    slave.input[0:4], 
                    byteorder='little', 
                    signed=True
                )
                
                return position
            return None
        except Exception as e:
            print(f"Read failed: {str(e)}")
            return None

    def close(self):
        """Close EtherCAT connection"""
        self.master.state = pysoem.INIT_STATE
        self.master.write_state()
        self.master.close()

def main():
    # Create EtherCAT driver instance
    # Replace 'eth0' with your network interface name
    driver = EtherCATDriver('eth0')
    
    if driver.setup():
        try:
            # Read feedback in a loop
            for _ in range(1000):  # Read 100 times
                feedback = driver.read_feedback()
                if feedback is not None:
                    print(f"Position feedback: {feedback}")
                time.sleep(0.01)  # 10ms cycle time
        finally:
            driver.close()
    else:
        print("Failed to setup EtherCAT communication")

if __name__ == "__main__":
    main()
